import cv2
import threading
import jisuan as ji
from time import sleep
import serial as ser
import time as sleep

# 全局变量控制状态
exit_program = False
color_commands = []  # 存储颜色和数量命令
initial_coords = None  # 存储初始坐标

x1, y1 = 0, 0
def f(angles):
    print(angles)
    for result in angles:
        for key, value in result.items():
            command: str = '#T000P{}T1000!'.format(key)
            ser.write(command.encode())
            command = '#T000P{}T0000!'.format(key)
            print(command)
            print('转动0')
            x, y  = value
            comm = ji.ArmController.generate_arm_command(x, y)
            if comm is None:  # 检查是否返回None
                print(f"无法到达位置 ({x}, {y})，跳过")
                continue
            result = '{'+ command + comm + '#005P1500T5000!' + '}'
            print(result)
            print('力臂移动')
            sleep(1)

            c = '{' + command + '#001P1000T1000!#002P1500T1000!#003P1050T1000!#005P1500T0000!' + '}'
            print(c)
            print('抓取物品')
            sleep(1)

            c = '{'+ command + '#001P1000T000!#002P1500T0000!#003P1050T0000!#005P1200T1000!' + '}'
            print(c)
            print('放到指定位置')
            sleep(1)

            c = '{'+ command + '#001P1400T0000!#002P1700T0000!#003P1400T0000!#005P1500T0000!' + '}
            print(c)
            print('复位')
            sleep(1)
def camera_loop():
    global exit_program, color_commands, initial_coords

    cap = cv2.VideoCapture(1)
    if not cap.isOpened():
        print("无法打开摄像头")
        return

    while not exit_program:
        ret, frame = cap.read()
        if not ret:
            print("获取帧失败")
            break

        # 如果有初始坐标，在画面上显示
        if initial_coords:
            x, y = initial_coords
            cv2.circle(frame, (x, y), 5, (0, 255, 0), -1)
            cv2.putText(frame, f"Initial: ({x}, {y})", (x + 10, y - 10),
                        cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 255, 0), 2)

        # 创建Calculate实例并处理图像
        calc = ji.Calculate(frame, initial_coords[0] if initial_coords else 0,
                           initial_coords[1] if initial_coords else 0)

        if color_commands:
            # 获取颜色列表
            colors = [cmd[0] for cmd in color_commands]
            # 处理图像并获取结果
            angles, mask, processed_frame = calc.shibie(colors)


            # 显示处理后的图像
            cv2.imshow('Processed View', processed_frame)
            cv2.imshow('Color Mask', mask)
            f(angles)
            # 打印坐标信息
            if hasattr(calc, 'x') and hasattr(calc, 'y'):
                print(f"实时坐标: ({calc.x}, {calc.y})")
        else:
            processed_frame = frame

        # 显示原始画面
        cv2.imshow('Original View', frame)

        # 退出检测 (按ESC键)
        if cv2.waitKey(1) == 27:
            exit_program = True
            break

    # 清理资源
    cap.release()
    cv2.destroyAllWindows()


def command_listener():
    global exit_program, color_commands, initial_coords

    print("命令说明:")
    print("  1. 输入初始坐标: x y (例如: 100 200)")
    print("  2. 输入颜色和数量: color quantity (例如: red 3)")
    print("  3. 退出命令: q")
    print("  4. 执行计算: c-计算")

    while not exit_program:
        user_input = input("输入命令: ").strip().lower()

        # 处理特殊命令
        if user_input in ['q', 'c']:
            if user_input == 'q':
                exit_program = True
                print("退出程序命令已接收")
                break
            elif user_input == 'c' and initial_coords:
                # 执行计算
                x1, y1 = initial_coords
                for color, quantity in color_commands:
                    # 这里调用计算函数
                    result = ji.Calculate(color, x1, y1, quantity)
                    print(f"颜色 {color} 数量 {quantity} 计算结果: {result}")
                color_commands = []  # 清空命令列表
            continue

        # 处理初始坐标输入
        if initial_coords is None:
            parts = user_input.split()
            if len(parts) == 2:
                try:
                    x = int(parts[0])
                    y = int(parts[1])
                    initial_coords = (x, y)
                    print(f"初始坐标已设置为: ({x}, {y})")
                except ValueError:
                    print("坐标必须是数字")
            else:
                print("请输入坐标格式: x y")
            continue

        # 处理颜色和数量命令
        parts = user_input.split()
        if len(parts) == 2:
            color, quantity = parts
            try:
                quantity = int(quantity)
                if 1 <= quantity <= 9:
                    color_commands.append((color, quantity))
                    print(f"已添加命令: 颜色={color}, 数量={quantity}")
                else:
                    print("数量必须在1-9之间")
            except ValueError:
                print("数量必须是数字")
        else:
            print("无效的命令格式，请使用: 颜色 数量")


if __name__ == "__main__":
    ser = ser.Serial('COM7', 115200)
    if ser.isOpen():
        print("打开串口成功")
        print(ser.name)
    else:
        print("打开串口失败")
    cam_thread = threading.Thread(target=camera_loop)
    cam_thread.daemon = True
    cam_thread.start()

    command_listener()

    cam_thread.join()
    print("程序已安全退出")
